// Fluxions 3D Engine
// matrix4.hpp
// Copyright (C) 2000-2011 Jonathan Metzgar
// 
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
// 
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
// 
// You should have received a copy of the GNU General Public License along
// with this program; if not, write to the Free Software Foundation, Inc.,
// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
#ifndef FLUXIONS_MATRIX4_HPP
#define FLUXIONS_MATRIX4_HPP

#include "math.hpp"

namespace Fluxions
{
	template <typename X>
	class TMatrix4
	{
	public:
		union
		{
			// standard form representation
			struct
			{
				X _11, _12, _13, _14;
				X _21, _22, _23, _24;
				X _31, _32, _33, _34;
				X _41, _42, _43, _44;
			};
			
			// normal access
			struct
			{
				X m00, m01, m02, m03;
				X m10, m11, m12, m13;
				X m20, m21, m22, m23;
				X m30, m31, m32, m33;
			};			
			
			// transposed access
			struct
			{
				X t00, t10, t20, t30;
				X t01, t11, t21, t31;
				X t02, t12, t22, t32;
				X t03, t13, t23, t33;
			};
			
			// array form
			struct
			{
				X m[16];
			};
			
			// 2D array form
			struct
			{
				X mm[4][4];
			};
		};
		
		TMatrix4();
		TMatrix4(
			const X a00, const X a10, const X a20, const X a30,
			const X a01, const X a11, const X a21, const X a31,
			const X a02, const X a12, const X a22, const X a32,
			const X a03, const X a13, const X a23, const X a33);
		TMatrix4(const TMatrix4<X> & M);
		TMatrix4(const X M[4][4]);
		TMatrix4(const X M[16]);
		template <typename T> const TMatrix4<X> & operator = (const TMatrix4<T> & M);
		
		template <typename T> operator TMatrix4<T> () const;

		void LoadIdentity();
		template <typename T> void MultMatrix(const TMatrix4<T> & M);
		template <typename T> const TMatrix4<X> & operator *= (const TMatrix4<T> & M);
		template <typename T> const TMatrix4<X> & operator += (const TMatrix4<T> & M);
		template <typename T> const TMatrix4<X> & operator -= (const TMatrix4<T> & M);

		template <typename T> const TMatrix4<X> operator * (const TMatrix4<T> & M) const;
		template <typename T> const TMatrix4<X> operator + (const TMatrix4<T> & M) const;
		template <typename T> const TMatrix4<X> operator - (const TMatrix4<T> & M) const;
		
		template <typename T> const TMatrix4<X> & operator *= (const T x);
		template <typename T> const TMatrix4<X> & operator /= (const T x);
		
		template <typename T> void Rotate(T angle, T x, T y, T z);
		template <typename T> void Scale(T x, T y, T z);
		template <typename T> void Translate(T x, T y, T z);
		template <typename T> void Ortho(T left, T right, T bottom, T top, T nearVal, T farVal);
		template <typename T> void Ortho2D(T left, T right, T bottom, T top);
		template <typename T> void Frustum(T left, T right, T bottom, T top, T nearVal, T farVal);
		template <typename T> void Perspective(T fovy, T aspect, T nearVal, T farVal);
		
		const X Determinant() const;
		bool Invert();
		void Transpose();
		void Adjugate();
	};
	
	//////////////////////////////////////////////////////////////////
	// I M P L E M E N T A T I O N ///////////////////////////////////
	//////////////////////////////////////////////////////////////////
	template <typename X>
	TMatrix4<X>::TMatrix4()
	{
	}
	
	template <typename X>
	TMatrix4<X>::TMatrix4(
		const X a00, const X a10, const X a20, const X a30,
		const X a01, const X a11, const X a21, const X a31,
		const X a02, const X a12, const X a22, const X a32,
		const X a03, const X a13, const X a23, const X a33
		)
	{
		m00 = a00; m10 = a10; m20 = a20; m30 = a30;
		m01 = a01; m11 = a11; m21 = a21; m31 = a31;
		m02 = a02; m12 = a12; m22 = a22; m32 = a32;
		m03 = a03; m13 = a13; m23 = a23; m33 = a33;
	}

	template <typename X>
	TMatrix4<X>::TMatrix4(const TMatrix4<X> & M)
	{
		_11 = M._11; _21 = M._21; _31 = M._31; _41 = M._41;
		_12 = M._12; _22 = M._22; _32 = M._32; _42 = M._42;
		_13 = M._13; _23 = M._23; _33 = M._33; _43 = M._43;
		_14 = M._14; _24 = M._24; _34 = M._34; _44 = M._44;
	}
	
	template <typename X>
	TMatrix4<X>::TMatrix4(const X M[4][4])
	{
		for (int i = 0; i < 4; i++)
		{
			for (int j = 0; j < 4; j++)
			{
				mm[i][j] = M[j][i];
			}
		}
	}
	
	template <typename X>
	TMatrix4<X>::TMatrix4(const X M[16])
	{
		for (int i = 0; i < 16; i++)
		{
			m[i] = M[i];
		}
	}
	
	template <typename X>
	template <typename T>
	const TMatrix4<X> & TMatrix4<X>::operator = (const TMatrix4<T> & M)
	{
		_11 = M._11; _21 = M._21; _31 = M._31; _41 = M._41;
		_12 = M._12; _22 = M._22; _32 = M._32; _42 = M._42;
		_13 = M._13; _23 = M._23; _33 = M._33; _43 = M._43;
		_14 = M._14; _24 = M._24; _34 = M._34; _44 = M._44;
		return *this;
	}

	template <typename X>
	template <typename D>
	TMatrix4<X>::operator TMatrix4<D> () const
	{
		return TMatrix4<D> (m00, m10, m20, m30,
			m01, m11, m21, m31,
			m02, m12, m22, m32,
			m03, m13, m23, m33);
	}
	
	template <typename X>
	void TMatrix4<X>::LoadIdentity()
	{
		_11 = 1; _21 = 0; _31 = 0; _41 = 0;
		_12 = 0; _22 = 1; _32 = 0; _42 = 0;
		_13 = 0; _23 = 0; _33 = 1; _43 = 0;
		_14 = 0; _24 = 0; _34 = 0; _44 = 1;
	}
	
	template <typename X>
	template <typename T>
	void TMatrix4<X>::MultMatrix(const TMatrix4<T> & M)
	{
		TMatrix4<X> u;
		u.m00 = m00 * M.m00 + m01 * M.m10 + m02 * M.m20 + m03 * M.m30;
		u.m01 = m00 * M.m01 + m01 * M.m11 + m02 * M.m21 + m03 * M.m31;
		u.m02 = m00 * M.m02 + m01 * M.m12 + m02 * M.m22 + m03 * M.m32;
		u.m03 = m00 * M.m03 + m01 * M.m13 + m02 * M.m23 + m03 * M.m33;
		u.m10 = m10 * M.m00 + m11 * M.m10 + m12 * M.m20 + m13 * M.m30;
		u.m11 = m10 * M.m01 + m11 * M.m11 + m12 * M.m21 + m13 * M.m31;
		u.m12 = m10 * M.m02 + m11 * M.m12 + m12 * M.m22 + m13 * M.m32;
		u.m13 = m10 * M.m03 + m11 * M.m13 + m12 * M.m23 + m13 * M.m33;
		u.m20 = m20 * M.m00 + m21 * M.m10 + m22 * M.m20 + m23 * M.m30;
		u.m21 = m20 * M.m01 + m21 * M.m11 + m22 * M.m21 + m23 * M.m31;
		u.m22 = m20 * M.m02 + m21 * M.m12 + m22 * M.m22 + m23 * M.m32;
		u.m23 = m20 * M.m03 + m21 * M.m13 + m22 * M.m23 + m23 * M.m33;
		u.m30 = m30 * M.m00 + m31 * M.m10 + m32 * M.m20 + m33 * M.m30;
		u.m31 = m30 * M.m01 + m31 * M.m11 + m32 * M.m21 + m33 * M.m31;
		u.m32 = m30 * M.m02 + m31 * M.m12 + m32 * M.m22 + m33 * M.m32;
		u.m33 = m30 * M.m03 + m31 * M.m13 + m32 * M.m23 + m33 * M.m33;
		*this = u;
	}
	
	template <typename X>
	template <typename T>
	const TMatrix4<X> & TMatrix4<X>::operator *= (const TMatrix4<T> & M)
	{
		TMatrix4<X> u;
		u.m00 = m00 * M.m00 + m01 * M.m10 + m02 * M.m20 + m03 * M.m30;
		u.m01 = m00 * M.m01 + m01 * M.m11 + m02 * M.m21 + m03 * M.m31;
		u.m02 = m00 * M.m02 + m01 * M.m12 + m02 * M.m22 + m03 * M.m32;
		u.m03 = m00 * M.m03 + m01 * M.m13 + m02 * M.m23 + m03 * M.m33;
		u.m10 = m10 * M.m00 + m11 * M.m10 + m12 * M.m20 + m13 * M.m30;
		u.m11 = m10 * M.m01 + m11 * M.m11 + m12 * M.m21 + m13 * M.m31;
		u.m12 = m10 * M.m02 + m11 * M.m12 + m12 * M.m22 + m13 * M.m32;
		u.m13 = m10 * M.m03 + m11 * M.m13 + m12 * M.m23 + m13 * M.m33;
		u.m20 = m20 * M.m00 + m21 * M.m10 + m22 * M.m20 + m23 * M.m30;
		u.m21 = m20 * M.m01 + m21 * M.m11 + m22 * M.m21 + m23 * M.m31;
		u.m22 = m20 * M.m02 + m21 * M.m12 + m22 * M.m22 + m23 * M.m32;
		u.m23 = m20 * M.m03 + m21 * M.m13 + m22 * M.m23 + m23 * M.m33;
		u.m30 = m30 * M.m00 + m31 * M.m10 + m32 * M.m20 + m33 * M.m30;
		u.m31 = m30 * M.m01 + m31 * M.m11 + m32 * M.m21 + m33 * M.m31;
		u.m32 = m30 * M.m02 + m31 * M.m12 + m32 * M.m22 + m33 * M.m32;
		u.m33 = m30 * M.m03 + m31 * M.m13 + m32 * M.m23 + m33 * M.m33;
		*this = u;
		return *this;
	}
	
	template <typename X>
	template <typename T>
	const TMatrix4<X> & TMatrix4<X>::operator += (const TMatrix4<T> & M)
	{
		m00 += M.m00; m01 += M.m01; m02 += M.m02; m03 += M.m03;
		m10 += M.m10; m11 += M.m11; m12 += M.m12; m13 += M.m13;
		m20 += M.m20; m21 += M.m21; m22 += M.m22; m23 += M.m23;
		m30 += M.m30; m31 += M.m31; m32 += M.m32; m33 += M.m33;
		return *this;
	}
	
	template <typename X>
	template <typename T>
	const TMatrix4<X> & TMatrix4<X>::operator -= (const TMatrix4<T> & M)
	{
		m00 -= M.m00; m01 -= M.m01; m02 -= M.m02; m03 -= M.m03;
		m10 -= M.m10; m11 -= M.m11; m12 -= M.m12; m13 -= M.m13;
		m20 -= M.m20; m21 -= M.m21; m22 -= M.m22; m23 -= M.m23;
		m30 -= M.m30; m31 -= M.m31; m32 -= M.m32; m33 -= M.m33;
		return *this;
	}

	template <typename X>
	template <typename T> 
	const TMatrix4<X> TMatrix4<X>::operator * (const TMatrix4<T> & M) const
	{
		TMatrix4<X> u;
		u.m00 = m00 * M.m00 + m01 * M.m10 + m02 * M.m20 + m03 * M.m30;
		u.m01 = m00 * M.m01 + m01 * M.m11 + m02 * M.m21 + m03 * M.m31;
		u.m02 = m00 * M.m02 + m01 * M.m12 + m02 * M.m22 + m03 * M.m32;
		u.m03 = m00 * M.m03 + m01 * M.m13 + m02 * M.m23 + m03 * M.m33;
		u.m10 = m10 * M.m00 + m11 * M.m10 + m12 * M.m20 + m13 * M.m30;
		u.m11 = m10 * M.m01 + m11 * M.m11 + m12 * M.m21 + m13 * M.m31;
		u.m12 = m10 * M.m02 + m11 * M.m12 + m12 * M.m22 + m13 * M.m32;
		u.m13 = m10 * M.m03 + m11 * M.m13 + m12 * M.m23 + m13 * M.m33;
		u.m20 = m20 * M.m00 + m21 * M.m10 + m22 * M.m20 + m23 * M.m30;
		u.m21 = m20 * M.m01 + m21 * M.m11 + m22 * M.m21 + m23 * M.m31;
		u.m22 = m20 * M.m02 + m21 * M.m12 + m22 * M.m22 + m23 * M.m32;
		u.m23 = m20 * M.m03 + m21 * M.m13 + m22 * M.m23 + m23 * M.m33;
		u.m30 = m30 * M.m00 + m31 * M.m10 + m32 * M.m20 + m33 * M.m30;
		u.m31 = m30 * M.m01 + m31 * M.m11 + m32 * M.m21 + m33 * M.m31;
		u.m32 = m30 * M.m02 + m31 * M.m12 + m32 * M.m22 + m33 * M.m32;
		u.m33 = m30 * M.m03 + m31 * M.m13 + m32 * M.m23 + m33 * M.m33;
		return u;
	}
	
	template <typename X>
	template <typename T> 
	const TMatrix4<X> TMatrix4<X>::operator + (const TMatrix4<T> & M) const
	{
		TMatrix4<X> u;
		u.m00 = m00 + M.m00; u.m01 = m01 + M.m01; u.m02 = m02 + M.m02; u.m03 = m03 + M.m03;
		u.m10 = m10 + M.m10; u.m11 = m11 + M.m11; u.m12 = m12 + M.m12; u.m13 = m13 + M.m13;
		u.m20 = m20 + M.m20; u.m21 = m21 + M.m21; u.m22 = m22 + M.m22; u.m23 = m23 + M.m23;
		u.m30 = m30 + M.m30; u.m31 = m31 + M.m31; u.m32 = m32 + M.m32; u.m33 = m33 + M.m33;
		return u;
	}
	
	template <typename X>
	template <typename T> 
	const TMatrix4<X> TMatrix4<X>::operator - (const TMatrix4<T> & M) const
	{
		TMatrix4<X> u;
		u.m00 = m00 - M.m00; u.m01 = m01 - M.m01; u.m02 = m02 - M.m02; u.m03 = m03 - M.m03;
		u.m10 = m10 - M.m10; u.m11 = m11 - M.m11; u.m12 = m12 - M.m12; u.m13 = m13 - M.m13;
		u.m20 = m20 - M.m20; u.m21 = m21 - M.m21; u.m22 = m22 - M.m22; u.m23 = m23 - M.m23;
		u.m30 = m30 - M.m30; u.m31 = m31 - M.m31; u.m32 = m32 - M.m32; u.m33 = m33 - M.m33;
		return u;
	}
	
	template <typename X>
	template <typename T>
	const TMatrix4<X> & TMatrix4<X>::operator *= (const T x)
	{
		for (int i = 0; i < 16; i++)
			m[i] *= x;
		return *this;
	}
	
	template <typename X>
	template <typename T> 
	const TMatrix4<X> & TMatrix4<X>::operator /= (const T x)
	{
		for (int i = 0; i < 16; i++)
			m[i] /= x;
		return *this;
	}
	
	template <typename X>
	template <typename T> 
	void TMatrix4<X>::Rotate(T angle, T x, T y, T z)
	{
		TMatrix4<X> u = *this;
		angle = DegToRad(angle);
		
		X length = x * x + y * y + z * z;
		if (length > 1)
		{
			length = sqrt(length);
			x /= length;
			y /= length;
			z /= length;
		}
		
		X c = cos(angle);
		X s = sin(angle);
		
		m[0] = x * x * (1 - c) + c;
		m[1] = y * x * (1 - c) + z * s;
		m[2] = x * z * (1 - c) - y * s;
		m[3] = 0;
		
		m[4] = x * y * (1 - c) - z * s;
		m[5] = y * y * (1 - c) + c;
		m[6] = y * z * (1 - c) + x * s;
		m[7] = 0;
		
		m[8] = x * z * (1 - c) + y * s;
		m[9] = y * z * (1 - c) - x * s;
		m[10]= z * z * (1 - c) + c;
		m[11]= 0;
		
		m[12] = 0;
		m[13] = 0;
		m[14] = 0;
		m[15] = 1;
		
		MultMatrix(u);
	}
	
	template <typename X>
	template <typename T> 
	void TMatrix4<X>::Scale(T x, T y, T z)
	{
		TMatrix4<X> u = *this;
		
		_11 = x; _21 = 0; _31 = 0; _41 = 0;
		_12 = 0; _22 = y; _32 = 0; _42 = 0;
		_13 = 0; _23 = 0; _33 = z; _43 = 0;
		_14 = 0; _24 = 0; _34 = 0; _44 = 1;
		
		MultMatrix(u);
	}
	
	template <typename X>
	template <typename T> 
	void TMatrix4<X>::Translate(T x, T y, T z)
	{
		TMatrix4<X> u = *this;
		
		_11 = 1; _21 = 0; _31 = 0; _41 = x;
		_12 = 0; _22 = 1; _32 = 0; _42 = y;
		_13 = 0; _23 = 0; _33 = 1; _43 = z;
		_14 = 0; _24 = 0; _34 = 0; _44 = 1;
		
		MultMatrix(u);
	}
	
	template <typename X>
	template <typename T> 
	void TMatrix4<X>::Ortho(T left, T right, T bottom, T top, T nearVal, T farVal)
	{
		TMatrix4<X> u = *this;
		
		X tx = - (right + left) / (right - left);
		X ty = - (top + bottom) / (top - bottom);
		X tz = - (farVal + nearVal) / (farVal - nearVal);
		
		_11 = 2 / (right - left); _21 = 0; _31 = 0; _41 = tx;
		_12 = 0; _22 = 2 / (top - bottom); _32 = 0; _42 = ty;
		_13 = 0; _23 = 0; _33 = -2 / (farVal - nearVal); _43 = tz;
		_14 = 0; _24 = 0; _34 = 0; _44 = 1;
		
		MultMatrix(u);
	}
	
	template <typename X>
	template <typename T> 
	void TMatrix4<X>::Ortho2D(T left, T right, T bottom, T top)
	{
		Ortho(left, right, bottom, top, -1, 1);
	}
	
	template <typename X>
	template <typename T> 
	void TMatrix4<X>::Frustum(T left, T right, T bottom, T top, T nearVal, T farVal)
	{
		TMatrix4<X> u = *this;
		
		X A = (right + left) / (right - left);
		X B = (top + bottom) / (top - bottom);
		X C = - (farVal + nearVal) / (farVal - nearVal);
		X D = - 2 * farVal * nearVal / (farVal - nearVal);
		
		_11 = 2 * nearVal / (right - left);
		_21 = 0; 
		_31 = A;
		_41 = 0;
		_12 = 0; _22 = 2 * nearVal / (top - bottom);
		_32 = B;
		_42 = 0;
		_13 = 0;
		_23 = 0;
		_33 = C;
		_43 = D;
		_14 = 0;
		_24 = 0;
		_34 = -1;
		_44 = 0;
		
		MultMatrix(u);
	}
	
	template <typename X>
	template <typename T> 
	void TMatrix4<X>::Perspective(T fovy, T aspect, T nearVal, T farVal)
	{
		TMatrix4<X> u = *this;
		
		T f = 1.0 / tan(fovy / 2);
		
		_11 = f / aspect;
		_21 = 0;
		_31 = 0;
		_41 = 0;
		_12 = 0;
		_22 = f;
		_32 = 0;
		_42 = 0;
		_13 = 0;
		_23 = 0;
		_33 = (farVal + nearVal) / (nearVal - farVal);
		_43 = 2 * farVal * nearVal / (nearVal - farVal);
		_14 = 0;
		_24 = 0;
		_34 = -1;
		_44 = 0;

		MultMatrix(u);
	}
	
	template <typename X>
	void TMatrix4<X>::Transpose()
	{
		TMatrix4<X> u = *this;
		
		m00 = u.t00; m10 = u.t10; u.m20 = u.t20; u.m30 = u.m30;
		m01 = u.t01; m11 = u.t11; u.m21 = u.t21; u.m31 = u.m31;
		m02 = u.t02; m12 = u.t12; u.m22 = u.t22; u.m32 = u.m32;
		m03 = u.t03; m13 = u.t13; u.m23 = u.t23; u.m33 = u.m33;
	}
	
	template <typename X>
	const X TMatrix4<X>::Determinant() const
	{
		X det;
		X r0, r1, r2, r3;
		
		// quick optimization
		X m22m33 = m22 * m33;
		X m32m23 = m32 * m23;
		X m12m33 = m12 * m33;
		X m32m13 = m32 * m13;
		X m12m23 = m12 * m23;
		X m22m13 = m22 * m13;
		X m02m33 = m02 * m33;
		X m02m23 = m02 * m23;
		X m32m03 = m32 * m03;
		X m22m03 = m33 * m03;
		X m02m13 = m02 * m13;
		X m12m03 = m12 * m03;
		
		r0 = 0;
		r0 += (m11 * (m22m33 - m32m23));
		r0 -= (m21 * (m12m33 - m32m13));
		r0 += (m31 * (m12m23 - m22m13));
		
		r1 = 0;
		r1 += (m01 * (m22m33 - m32m23));
		r1 -= (m21 * (m02m33 - m32m03));
		r1 += (m31 * (m02m23 - m22m03));
		
		r2 = 0;
		r2 += (m01 * (m12m33 - m32m13));
		r2 -= (m11 * (m02m33 - m32m03));
		r2 += (m31 * (m02m13 - m12m03));
		
		r3 = 0;
		r3 += (m01 * (m12m23 - m22m13));
		r3 -= (m11 * (m02m23 - m22m03));
		r3 += (m21 * (m02m13 - m12m03));

		det = 0;
		det += m00 * r0;
		det -= m10 * r1;
		det += m20 * r2;
		det -= m30 * r3;
		
		return det;
	}
	
	template <typename X>
	void TMatrix4<X>::Adjugate()
	{
		X c00, c10, c20, c30;
		X c01, c11, c21, c31;
		X c02, c12, c22, c32;
		X c03, c13, c23, c33;
		
		X m02m13 = m02 * m13;
		X m02m23 = m02 * m23;
		X m02m33 = m02 * m33;
		X m12m03 = m12 * m03;
		X m12m23 = m12 * m23;
		X m12m33 = m12 * m33;
		X m22m03 = m22 * m03;
		X m22m13 = m22 * m13;
		X m22m33 = m22 * m33;
		X m32m03 = m32 * m03;
		X m32m13 = m32 * m13;
		X m32m23 = m32 * m23;

		X m01m13 = m01 * m13;
		X m01m23 = m01 * m23;
		X m01m33 = m01 * m33;
		X m11m03 = m11 * m03;
		X m11m23 = m11 * m23;
		X m11m33 = m11 * m33;
		X m21m03 = m21 * m03;
		X m21m13 = m21 * m13;
		X m21m33 = m21 * m33;
		X m31m03 = m31 * m03;
		X m31m13 = m31 * m13;
		X m31m23 = m31 * m23;
		
		
		X m01m12 = m01 * m12;
		X m01m22 = m01 * m22;
		X m01m32 = m01 * m32;
		X m11m02 = m11 * m02;
		X m11m22 = m11 * m22;
		X m11m32 = m11 * m32;
		X m21m02 = m21 * m02;
		X m21m12 = m21 * m12;
		X m21m32 = m21 * m32;
		X m31m02 = m31 * m02;
		X m31m12 = m31 * m12;
		X m31m22 = m31 * m22;
		
		c00 = 0;
		c00 += (m11 * (m22m33 - m32m23));
		c00 -= (m21 * (m12m33 - m32m13));
		c00 += (m31 * (m12m33 - m32m13));

		c10 = 0;
		c10 -= (m01 * (m22m33 - m32m23));
		c10 += (m21 * (m02m33 - m32m03));
		c10 -= (m31 * (m02m23 - m22m03));
		
		c20 = 0;
		c20 += (m01 * (m12m33 - m32m13));
		c20 -= (m11 * (m02m33 - m32m03));
		c20 += (m31 * (m02m13 - m12m03));
		
		c30 = 0;
		c30 -= (m01 * (m12m33 - m22m13));
		c30 += (m11 * (m02m23 - m22m03));
		c30 -= (m21 * (m02m13 - m12m03));
		
		c01 = 0;
		c01 -= (m10 * (m22m33 - m32m23));
		c01 += (m20 * (m12m33 - m32m13));
		c01 -= (m30 * (m12m23 - m32m13));

		c11 = 0;
		c11 += (m00 * (m22m33 - m32m23));
		c11 -= (m20 * (m02m33 - m32m03));
		c11 += (m30 * (m02m23 - m22m03));
		
		c21 = 0;
		c21 -= (m00 * (m12m33 - m32m13));
		c21 += (m10 * (m02m33 - m32m03));
		c21 -= (m30 * (m02m13 - m12m03));
		
		c31 = 0;
		c31 += (m00 * (m12m23 - m22m13));
		c31 -= (m10 * (m02m23 - m22m03));
		c31 += (m20 * (m02m13 - m12m03));
		
		c02 = 0;
		c02 += (m10 * (m21m33 - m31m23));
		c02 -= (m20 * (m11m33 - m31m13));
		c02 += (m30 * (m11m23 - m21m13));

		c12 = 0;
		c12 -= (m00 * (m21m33 - m31m23));
		c12 += (m20 * (m01m33 - m31m03));
		c12 -= (m30 * (m01m23 - m21m03));
		
		c22 = 0;
		c22 += (m00 * (m11m33 - m31m13));
		c22 -= (m10 * (m01m33 - m31m03));
		c22 += (m30 * (m01m13 - m11m03));
		
		c32 = 0;
		c32 -= (m00 * (m11m23 - m21m13));
		c32 += (m10 * (m01m23 - m21m03));
		c32 -= (m20 * (m01m13 - m11m03));
		
		c03 = 0;
		c03 -= (m10 * (m21m32 - m31m22));
		c03 += (m20 * (m11m32 - m31m12));
		c03 -= (m30 * (m11m22 - m21m12));

		c13 = 0;
		c13 += (m00 * (m21m32 - m31m22));
		c13 -= (m20 * (m01m32 - m31m02));
		c13 += (m30 * (m01m22 - m21m02));
		
		c23 = 0;
		c23 -= (m00 * (m11m32 - m31m12));
		c23 += (m10 * (m01m32 - m31m02));
		c23 -= (m30 * (m01m12 - m11m02));
		
		c33 = 0;
		c33 += (m00 * (m11m22 - m21m12));
		c33 -= (m10 * (m01m22 - m21m02));
		c33 += (m20 * (m01m12 - m11m02));
		
		m00 = c00;
		m10 = c01;
		m20 = c02;
		m30 = c03;
		
		m01 = c10;
		m11 = c11;
		m21 = c12;
		m31 = c13;
		
		m02 = c20;
		m12 = c21;
		m22 = c22;
		m32 = c23;
		
		m03 = c30;
		m13 = c31;
		m23 = c32;
		m33 = c33;
	}
	
	template <typename X>
	bool TMatrix4<X>::Invert()
	{
		X det;
		
		det = Determinant();
		if (det == 0)
			return false;
			
		Adjugate();
		Mul(1 / det);
		
		return true;
	}

	typedef TMatrix4<float> Matrix4f;
	typedef TMatrix4<double> Matrix4d;
}

#endif
